#include <math.h>
#include "global.h"
#include "WARDEN.h"


Coordinate Origin;
Coordinate bounds [BOUND_POINTS];


Warden::Warden(){

}

Warden::Warden(Coordinate orgn){
	Origin = orgn;
}

void Warden::createBoundsArray ()
	{
    	bounds[0].lat = 42.39388815;
		bounds[0].lon = -72.52890826;

    	bounds[1].lat = 42.39365914;
		bounds[1].lon = -72.52965247;

    	bounds[2].lat = 42.39332142;
		bounds[2].lon = -72.52946627;

    	bounds[3].lat = 42.39358300;
		bounds[3].lon = -72.52873163;
    }


bool dubEq(double a, double b){

	double diff=0;
	diff = a-b;
	if( (diff<(.0001)) && (diff>(-.0001)) ) return true;
	else return false;

}

bool Warden::onSameSide (Coordinate test)
{
	double lat = test.lat;
	double lon = test.lon;
	Coordinate bounds1, bounds2;
	double a, b, c, d, x1, y1;
	bool given = false, testry=false, inbounds=true;

	for(int i=0; i<BOUND_POINTS; i++)
	{
		bounds1 = bounds[i];
		if(i==BOUND_POINTS-1)
		{
			bounds2 = bounds[0];
		}
		else
		{
			bounds2 = bounds[i+1];
		}
			
		a = bounds1.lon;
		b = bounds1.lat;
		c = bounds2.lon;
		d = bounds2.lat;
		x1 = Origin.lon;
		y1 = Origin.lat;
			
		given = y1-b<(d-b)/(c-a)*(x1-a);
		testry = lat-b<(d-b)/(c-a)*(lon-a);
			
		if (given != testry)
		{
			inbounds = false;
		}
			
	}
	return inbounds;
}


bool Warden::inBounds (Coordinate test)
    {
    	Coordinate bounds1, bounds2;
    	int boundsCrossed = 0;
    	int i;
    	for(i=0; i<BOUND_POINTS; i++)
    	{
    		bounds1 = bounds[i];
    		if(i==BOUND_POINTS-1)
    		{
    			bounds2 = bounds[0];
    		}
    		else
    		{
    			bounds2 = bounds[i+1];
    		}

    		/*
    		* This "if" is to test whether the two lines are parallel, because
    		* if they are, their intersection will not be valid
			*/
    		if(true)   // ((Origin.lat()-test.lat()) / (Origin.lon()-test.lon())) != (y3-y4)/(x3-x4)) )
    		{
    			Coordinate intersect = getIntersection(test, bounds1, bounds2);
    			if(intersect.lon==bounds1.lon && intersect.lat==bounds1.lat)
				//if(dubEq(intersect.lon(), bounds1.lon()) && dubEq(intersect.lat(), bounds1.lat()))
				{
					/*
					 * This if is put in place so that in the case that the line segemnt
					 * from origin to test point runs through a boundary point, that
					 * intersection will be counted only once and not twice. When this point
					 * assumes the role of bounds2 (every point gets to be both bounds1
					 * and bounds2 at some point) then this intersection will be counted.
					 * There is also the chance that the intersection of lines will occur on
					 * a boundary point with the test point inside the bounds. While this will
					 * return out of bounds, we will take multiple readings before deciding
					 * that the collar is truly out of bounds, and these multiple readings
					 * will each be slightly different.
					 */
				}
				else if(intersectionValid(intersect, test, bounds1, bounds2))
				{
					boundsCrossed++;
				}
    		}
    	}
    	///printf("Bounds Crossed: %d\n", boundsCrossed);
    	if(boundsCrossed%2 == 0)
		{
			digitalWrite(4, HIGH);
			return true;
		}

		return false;
    }


    /*
     * This method takes in 3 points (and can reference the origin point) and returns
     * the intersection of the line formed by the origin and test with the line formed
     * by the two bounds points
     */
Coordinate Warden::getIntersection (Coordinate test, Coordinate b1, Coordinate b2)
	{
		double x1,x2,x3,x4,y1,y2,y3,y4,x,y;

		/*
		 * There are five possible cases:
		 * 1) The boundary line is vertical, the test-origin line is horizontal
		 * 2) The boundary line is vertical, the test-origin line isn't horizontal
		 * 3) The test-origin line is vertical, the boundary line is horizontal
		 * 4) The test-origin line is vertical, the boundary line isn't horizontal
		 * 5) Neither line is vertical
		 *
		 * Here we deal with each of these cases. We don't care if the lines are parallel,
		 * because they will never intersect.
		 */
		if(b1.lon==b2.lon)
		{
			/*
			 * Case 1: boundary vertical, test-origin horizontal
			 * Solution: we know the intersection easily
			 */
			if(Origin.lat==test.lat)
			{
				x = b1.lon;
				y = Origin.lat;
			}

			/*
			 * Case 2: boundary vertical, test-origin not horizontal
			 * Solution: flip the axes so there are no vertical lines
			 */
			else
			{
				y1 = Origin.lon;
				y2 = test.lon;
				y3 = b1.lon;
				y4 = b2.lon;
				x1 = Origin.lat;
				x2 = test.lat;
				x3 = b1.lat;
				x4 = b2.lat;

				y = (x1*(y2-y1)/(x2-x1)-x3*(y4-y3)/(x4-x3)+y3-y1)/((y2-y1)/(x2-x1)-(y4-y3)/(x4-x3));
				x = (y2-y1)/(x2-x1)*(y-x1)+y1;
			}
		}
		else if(Origin.lon==test.lon)
		{
			/*
			 * Case 3: test-origin vertical, boundary horizontal
			 * Solution: we know the intersection easily
			 */
			if(b1.lat==b2.lat)
			{
				x = Origin.lon;
				y = b1.lat;
			}

			/*
			 * Case 4: test-origin vertical, boundary not horizontal
			 * Solution: flip axes so there are no vertical lines
			 */
			else
			{
				y1 = Origin.lon;
				y2 = test.lon;
				y3 = b1.lon;
				y4 = b2.lon;
				x1 = Origin.lat;
				x2 = test.lat;
				x3 = b1.lat;
				x4 = b2.lat;

				y = (x1*(y2-y1)/(x2-x1)-x3*(y4-y3)/(x4-x3)+y3-y1)/((y2-y1)/(x2-x1)-(y4-y3)/(x4-x3));
				x = (y2-y1)/(x2-x1)*(y-x1)+y1;
			}
		}

		/*
		 * Case 5: neither line is vertical
		 * Solution: use the intersection of two lines formula
		 */
		else
		{
			x1 = Origin.lon;
			x2 = test.lon;
			x3 = b1.lon;
			x4 = b2.lon;
			y1 = Origin.lat;
			y2 = test.lat;
			y3 = b1.lat;
			y4 = b2.lat;

			x = ( ((x1*(y2-y1))/(x2-x1)) - ( ((x3*(y4-y3))/(x4-x3))+y3-y1 ))   /   ( ((y2-y1)/(x2-x1))-((y4-y3)/(x4-x3)) );
			y = (y2-y1)/(x2-x1)*(x-x1)+y1;
		}

		Coordinate returnCoord;
		returnCoord.lat = x;
		returnCoord.lon = y;

		return returnCoord;
	}

	/*
	 * This method takes a given intersection of two lines and checks whether the intersection
	 * occurs within the two lines segments formed by the four points
	 */
bool Warden::intersectionValid (Coordinate intersection, Coordinate test, Coordinate b1, Coordinate b2)
	{
		double x1 = Origin.lon;
		double x2 = test.lon;
		double x3 = b1.lon;
		double x4 = b2.lon;
		double y1 = Origin.lat;
		double y2 = test.lat;
		double y3 = b1.lat;
		double y4 = b2.lat;
		double interX = intersection.lon;
		double interY = intersection.lat;

		/*
		 * A bit of craziness: just found a bug with this. If the line drawn from
		 * the origin to the test point intersects the bounds AT A BOUNDARY POINT,
		 * then the program will think it has crossed two lines and call it inbounds.
		 * I fixed this with an "if" statement in the "inbounds" method. It executes
		 * no code and has a comment explaining its purpose.
		 */

		if(x1>=x2)
		{
			if(!(x1>=interX && interX>=x2))
			{
				return false;
			}
		}
		else
		{
			if(!(x2>=interX && interX>=x1))
			{
				return false;
			}
		}
		if(x3>=x4)
		{
			if(!(x3>=interX && interX>=x4))
			{
				return false;
			}
		}
		else
		{
			if(!(x4>=interX && interX>=x3))
			{
				return false;
			}
		}


		if(y1>=y2)
		{
			if(!(y1>=interY && interY>=y2))
			{
				return false;
			}
		}
		else
		{
			if(!(y2>=interY && interY>=y1))
			{
				return false;
			}
		}
		if(y3>=y4)
		{
			if(!(y3>=interY && interY>=y4))
			{
				return false;
			}
		}
		else
		{
			if(!(y4>=interY && interY>=y3))
			{
				return false;
			}
		}
		return true;
	}

double Warden::shortestDistance (Coordinate test)
	{
		int i;
		double Ax, Bx, Cx, Ay, By, Cy, a, b, c, r;
		Coordinate bounds1, bounds2;
		double tempDistance;
		double distance = 999999999;

		/*
		 * Iterate through sets of adjacent boundary points
		 */
		for(i=0; i<BOUND_POINTS; i++)
		{
			bounds1 = bounds[i];
			if(i==BOUND_POINTS-1)
			{
				bounds2 = bounds[0];
			}
			else
			{
				bounds2 = bounds[i+1];
			}
			Cx = test.lon;
			Ax = bounds1.lon;
			Bx = bounds2.lon;
			Cy = test.lat;
			Ay = bounds1.lat;
			By = bounds2.lat;

			/*
			 * r represents where the perpendicular intersection of the test point intersects the line segment formed by
			 * the two boundary points. If r<0 the intersection occurs outside of bounds1. If r>1 the intersection
			 * occurs outside of bounds2. If 0<r<1 the intersection occurs on the line segment.
			 */
			r = ((Cx-Ax)*(Bx-Ax)+(Cy-Ay)*(By-Ay))/pow(sqrt(pow((Bx-Ax),2)+pow((By-Ay),2)),2);

			if (0<r && r<1)
			{
				/*
				 * This tests whether the given boundary line segment is a vertical line. If it is, we swap all x and y
				 * coordinates for both boundary points and the test point. This allows us to sidestep the vertical line
				 * problem and calculate the accurate distance.
				 */
				if(Ax==Bx)
				{
					a = (Bx-Ax)/(By-Ay);
					b = -1;
					c = Ax - ((Bx-Ax)/(By-Ay))*Ay;
					Cx = test.lat;
					Cy = test.lon;
				}
				else
				{
					a = (By-Ay)/(Bx-Ax);
					b = -1;
					c = Ay - ((By-Ay)/(Bx-Ax))*Ax;
				}

				/*
				 * The absolute value function was truncating our doubles to integers, so we implemented our own here.
				 * We use the point-line distance formula.
				 */
				if(a*Cy+b*Cx+c >= 0)
				{
					tempDistance = (a*Cx+b*Cy+c)/sqrt(a*a+b*b);
				}
				else
				{
					tempDistance = -1*(a*Cx+b*Cy+c)/sqrt(a*a+b*b);
				}

				/*
				 * We only want to consider positive distances- otherwise the smallest distance will be the greatest
				 * negative number
				 */
				if(tempDistance < 0)
				{
					tempDistance *= -1;
				}

				if (distance>tempDistance)
				{
					distance=tempDistance;
				}
			}
			/*
			 * If the perpendicular intersection occurs outside of the line segment, we calculate the distance to
			 * each of the two boundary points.
			 */
			else
			{
				tempDistance = sqrt(pow((Cx-Ax),2)+pow((Cy-Ay),2));
				if (distance>tempDistance)
				{
					distance=tempDistance;
				}

				tempDistance = sqrt(pow((Cx-Bx),2)+pow((Cy-By),2));
				if (distance>tempDistance)
				{
					distance=tempDistance;
				}
			}

		}
		return distance;
}